#include "v4l2.h"

//打印16进制unsigned char* data
void VideoDevice::print_rgb_data_as_hex(unsigned char* rgb_data) {
    int length = iHeight * iWidth * 3;
    for (int i = 0; i < length; i++) {
        printf("%02X ", rgb_data[i]);  // 打印每个字节的16进制表示
        if ((i + 1) % 16 == 0) {       // 每行打印16个字节
            printf("\n");
        }
    }
    printf("\n");  // 打印结束后换行
}

// 分配RGB缓冲区
void VideoDevice::new_rgb_data()
{
    int i = 0;
    for(; i < NB_BUFFER; i++)
    {
        rgb_data[i] = new unsigned char[iWidth * iHeight * 3];// RGB 每个像素3个字节
    }
}

int VideoDevice::v4l2_video_handle()
{
#ifdef YUYV
    // 获取当前缓冲区数据
    unsigned char* yuv_data = pucVideBuf[iVideoBufCurIndex];

    // 将YUYV转换为RGB
    yuyv_to_rgb(yuv_data, rgb_data[iVideoBufCurIndex], iWidth, iHeight);

    save_rgb_video("output.mp4", rgb_data[iVideoBufCurIndex], 30, 200, iWidth, iHeight);

    // 保存RGB数据为图片文件
    switch(iVideoBufCurIndex)
    {
        case 0:
            save_rgb_image("output0.jpg", rgb_data[iVideoBufCurIndex], iWidth, iHeight);
            break;
        case 1:
            save_rgb_image("output1.jpg", rgb_data[iVideoBufCurIndex], iWidth, iHeight);
            break;
        case 2:
            save_rgb_image("output2.jpg", rgb_data[iVideoBufCurIndex], iWidth, iHeight);
            break;
    }
    // print_rgb_data_as_hex(rgb_data[iVideoBufCurIndex]);
#elif defined(MJPEG)
    unsigned char* jpg_data = pucVideBuf[iVideoBufCurIndex];
    save_rgb_video("output.avi", jpg_data, 30, 200, iWidth, iHeight);
    // print_rgb_data_as_hex(pucVideBuf[iVideoBufCurIndex]);
#else
#endif
    
    // image_show("output.jpg");
    return 0;
}

void VideoDevice::image_show(const char* filename){
    cv::Mat image = cv::imread(filename, cv::IMREAD_COLOR);

    cv::imshow("Disply",image);

    cv::waitKey(0);

    cv::destroyAllWindows();
}

void VideoDevice::save_rgb_video(const char* filename, unsigned char* rgb_data, int fps, int number_fps, int width, int height){
    if(number == number_fps){
        return;
    }

    // 如果 VideoWriter 未初始化，则初始化并打开文件
    if (!writer.isOpened()) {
#ifdef YUYV
        writer.open(filename, cv::VideoWriter::fourcc('m', 'p', '4', 'v'), fps, cv::Size(width, height));
#elif defined(MJPEG)
        writer.open(filename, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'), fps, cv::Size(width, height));
#else
#endif
        if (!writer.isOpened()) {
            std::cerr << "无法打开视频文件: " << filename << std::endl;
            return;
        }
    }

#ifdef YUYV
    cv::Mat rgb_img(height, width, CV_8UC3, rgb_data);
    writer.write(rgb_img);
#elif defined(MJPEG)
    std::vector<unsigned char> data_vec(rgb_data, rgb_data + iVideoBufMaxLen); // iVideoBufMaxLen为JPEG数据的长度
    cv::Mat decoded_img = cv::imdecode(data_vec, cv::IMREAD_COLOR);  // 解码 MJPEG 数据
    if (decoded_img.empty()) {
        std::cerr << "MJPEG 解码失败" << std::endl;
        return;
    }
    writer.write(decoded_img);

    // 保存RGB数据为图片文件
    switch(iVideoBufCurIndex)
    {
        case 0:
            cv::imwrite("output0.jpg", decoded_img);
            break;
        case 1:
            cv::imwrite("output1.jpg", decoded_img);
            break;
        case 2:
            cv::imwrite("output2.jpg", decoded_img);
            break;
    }
#else
#endif

    number++;
    if(number == number_fps){
        writer.release();
        std::cout << "视频保存完成: " << filename << std::endl;
    }
}

void VideoDevice::save_rgb_image(const char* filename, unsigned char* rgb_data, int width, int height) {
    // 将 RGB 数据转换为 cv::Mat 对象
    cv::Mat image(height, width, CV_8UC3, rgb_data);

    // 将图像保存为文件
    cv::imwrite(filename, image);
}

void VideoDevice::yuyv_to_rgb(unsigned char* yuyv_data, unsigned char* rgb_data, int width, int height) {
    // 创建 YUYV 图像的 cv::Mat 对象
    cv::Mat yuyv_img(height, width, CV_8UC2, yuyv_data);

    // 创建 RGB 图像的 cv::Mat 对象
    cv::Mat rgb_img(height, width, CV_8UC3, rgb_data);

    // 转换 YUYV 到 RGB
    cv::cvtColor(yuyv_img, rgb_img, cv::COLOR_YUV2BGR_YUYV);
}

void VideoDevice::resizeRGB(unsigned char* rgb_data, int width, int height, unsigned char* resized_rgb_data, int new_width, int new_height) {
    // 目标图像的尺寸
    cv::Size dst_size(new_width, new_height);

    // 创建OpenCV Mat对象
    cv::Mat src(height, width, CV_8UC3, rgb_data);

    // 创建目标尺寸的Mat对象
    cv::Mat dst(new_height, new_width, CV_8UC3, resized_rgb_data);

    // 调整大小
    cv::resize(src, dst, dst_size);
}

int VideoDevice::clamp(int value) {
    if (value < 0) return 0;
    if (value > 255) return 255;
    return value;
}

void VideoDevice::yuyv_to_rgb2(unsigned char *yuv_data, unsigned char *rgb_data, int width, int height) {
    int y0, y1, u, v;
    int r, g, b;
    int c, d, e;
    for (int i = 0; i < width * height * 2; i += 4) {
        y0 = yuv_data[i];
        u  = yuv_data[i + 1] - 128;
        y1 = yuv_data[i + 2];
        v  = yuv_data[i + 3] - 128;

        c = y0 - 16;
        d = u;
        e = v;

        r = clamp(( 298 * c           + 409 * e + 128) >> 8);
        g = clamp(( 298 * c - 100 * d - 208 * e + 128) >> 8);
        b = clamp(( 298 * c + 516 * d           + 128) >> 8);

        rgb_data[i * 3 / 2] = r;
        rgb_data[i * 3 / 2 + 1] = g;
        rgb_data[i * 3 / 2 + 2] = b;

        c = y1 - 16;

        r = clamp(( 298 * c           + 409 * e + 128) >> 8);
        g = clamp(( 298 * c - 100 * d - 208 * e + 128) >> 8);
        b = clamp(( 298 * c + 516 * d           + 128) >> 8);

        rgb_data[i * 3 / 2 + 3] = r;
        rgb_data[i * 3 / 2 + 4] = g;
        rgb_data[i * 3 / 2 + 5] = b;
    }
}
